/*
 *  Quaternion.h
 *  GLRender
 *
 *  Created by Patrick zulian on 5/9/10.
 *  Copyright 2010 University  of Lugano. All rights reserved.
 *
 */

#ifndef QUATERNION_H
#define QUATERNION_H

#include "Point3.h"
#include "Matrix4.h"

template<typename T>
class Quaternion {
private:
	T values[4];
	
	friend 
	void operator << (Point3<T> &p, const Quaternion &q)
	{
		for (int i = 0; i < 3; i++) {
			p[i] = q[i];
		}
	}
	
public:
	static const int X = 0, Y = 1, Z = 2, W = 3;
	Quaternion(const T x = (T) 0.0, const T y = (T) 0.0, const T z = (T) 0.0, const T w = (T) 0.0)
	{
		values[X] = x; values[Y] = y; values[Z] = z; values[W] = w;
	}
	
	//Quaternion()
//	{
//	}
	
	virtual
	~Quaternion() { }
	
	void zeros()
	{
		for(int i = 0; i < 4; i++) {
			values[i] = 0;
		}
	}
	
	inline
	const T operator()(const int i) const
	{
		return values[i];
	}
	
	inline
	T & operator()(const int i)
	{
		return values[i];
	}
	
	inline
	const T operator[](const int i) const
	{
		return values[i];
	}
	
	inline
	T & operator[](const int i) 
	{
		return values[i];
	}
	
	inline
	void normalize()
	{
		T _norm = norm();
		if (_norm == (T) 0.0) {
			return;
		}
		
		for (int i = 0; i < 4; i++) {
			values[i] /= _norm;
		}
	}
	
	inline
	T norm() const
	{
		return (T) sqrt( values[X] * values[X] + values[Y] * values[Y] + values[Z] * values[Z] + values[W] * values[W] );
	}
		
	inline
	Quaternion getConjugate() const 
	{
		return Quaternion(-values[X], -values[Y], -values[Z], values[W]);
	}
	
	Quaternion operator * (const Quaternion &other) const 
	{
		return Quaternion(values[W] * other(X) + values[X] * other(W) + values[Y] * other(Z) - values[Z] * other(Y),
						  values[W] * other(Y) + values[Y] * other(W) + values[Z] * other(X) - values[X] * other(Z),
						  values[W] * other(Z) + values[Z] * other(W) + values[Z] * other(Y) - values[Y] * other(X),
						  values[W] * other(W) + values[X] * other(X) + values[Y] * other(Y) - values[Z] * other(Z));
	}
	
	///Returns a rotated vector
	Point3<T> operator * (const Point3<T> &point) const
	{
		Point3<T> p = point;
		p.normalize();
		Quaternion q; q << p;
		Point3<T> ret; ret << (*this) * q * getConjugate();
		return ret;
	}
	
	
	void operator << (const Point3<T> &point)
	{
		values[X] = point.x();
		values[Y] = point.y();
		values[Z] = point.z();
		values[W] = (T) 0.0;
	}
	
	void rotate(const T theta, const Point3<T> &axis)
	{
		T angle = theta * (T) 0.5; 
		T sangle = (T) sin(angle);
		
		assert(axis.norm() == (T) 1.0);
		
		for (int i = 0; i < 3; i++) {
			(*this)[i] = axis[i]*sangle;
		}
		
		(*this)[W] = (T) cos(angle);
	}
	
	//radians
	void formEulerAngles(const T pitch, const T yaw, const T roll)
	{
		const T p = pitch/(T)2.0, y = yaw/(T)2.0, r = roll/(T)2.0;
		const T sp = (T) sin(p), sy = (T) sin(y), sr = (T) sin(r);
		const T cp = (T) cos(p), cy = (T) cos(y), cr = (T) cos(r);
		(*this)(X) = sr * cp * cy - cr * sp * sy;
		(*this)(Y) = cr * sp * cy + sr * cp * sy;
		(*this)(Z) = cr * cp * sy - sr * sp * cy;
		(*this)(W) = cr * cp * cy + sr * sp * sy;
		
		normalize();
	}
	
	Matrix4<T> asMatrix() const 
	{
		const T x2 = (*this)(X) * (*this)(X), y2 = (*this)(Y) * (*this)(Y), z2 = (*this)(Z) * (*this)(Z);
		const T xy = (*this)(X) * (*this)(Y), xz = (*this)(X) * (*this)(Z), yz = (*this)(Y) * (*this)(Z);
		const T wx = (*this)(W) * (*this)(X), wy = (*this)(W) * (*this)(Y), wz = (*this)(W) * (*this)(Z);
		Matrix4<T> M;
		
		//row major
		M(0,0) = (T) 1.0 - 2.0 * (y2 - z2); M(0,1) = (T) 2.0 * (xy + wz);		M(0,2) = (T) 2.0 * (xz - wy);
		M(1,0) = (T) 2.0 * (xy - wz);		M(1,1) = (T) 1.0 - 2.0 + (x2 + z2);	M(1,2) = (T) 2.0 * (yz + wx);
		M(2,0) = (T) 2.0 * (xz + wy);		M(2,1) = (T) 2.0 * (yz - wx);		M(2,2) = (T) 1.0 - 2.0 * (x2 + y2);
		return M;
	}
	
    void asGLMatrix(T ** M) const 
	{
		const T x2 = (*this)(X) * (*this)(X), y2 = (*this)(Y) * (*this)(Y), z2 = (*this)(Z) * (*this)(Z);
		const T xy = (*this)(X) * (*this)(Y), xz = (*this)(X) * (*this)(Z), yz = (*this)(Y) * (*this)(Z);
		const T wx = (*this)(W) * (*this)(X), wy = (*this)(W) * (*this)(Y), wz = (*this)(W) * (*this)(Z);

		//column major NO HOMOGENEOUS COORINATES
		M[0][0] = (T) 1.0 - 2.0 * (y2 - z2); M[1][0] = (T) 2.0 * (xy + wz);			M[2][0] = (T) 2.0 * (xz - wy);
		M[0][1] = (T) 2.0 * (xy - wz);		 M[1][1] = (T) 1.0 - 2.0 + (x2 + z2);	M[2][1] = (T) 2.0 * (yz + wx);
		M[0][2] = (T) 2.0 * (xz + wy);		 M[1][2] = (T) 2.0 * (yz - wx);			M[2][2] = (T) 1.0 - 2.0 * (x2 + y2);
	
	}
	/// array OUT, size=4x4=16 
	void asGLArray(T * array) const 
	{
		const T x2 = (*this)(X) * (*this)(X), y2 = (*this)(Y) * (*this)(Y), z2 = (*this)(Z) * (*this)(Z);
		const T xy = (*this)(X) * (*this)(Y), xz = (*this)(X) * (*this)(Z), yz = (*this)(Y) * (*this)(Z);
		const T wx = (*this)(W) * (*this)(X), wy = (*this)(W) * (*this)(Y), wz = (*this)(W) * (*this)(Z);
		
		//column major 
		array[0] = (T) 1.0 - 2.0 * (y2 - z2); array[4] = (T) 2.0 * (xy + wz);			array[8] = (T) 2.0 * (xz - wy);
		array[1] = (T) 2.0 * (xy - wz);		  array[5] = (T) 1.0 - 2.0 + (x2 + z2);		array[9] = (T) 2.0 * (yz + wx);
		array[2] = (T) 2.0 * (xz + wy);		  array[6] = (T) 2.0 * (yz - wx);			array[10] = (T) 1.0 - 2.0 * (x2 + y2);
		
		//homogeneous coordinates RESETS TRANSLATION VECTOR b
		array[3] = array[7] = array[11] = array[12] = array[13] = array[14] =  (T) 0.0;
		
		array[15] = (T) 1.0;
		
	}
	
};















#endif
